package br.usjt.mazerobot;

import br.usjt.mazerobot.comunicacao.Comunicador;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;

public class LegoNXTRobo implements Robo {

    //Atributo responsável por controlar a volta do sensor ultrasonico.
    //Ao girar o sensor sentido horário, o contador será incrementado de 1.
    //Ao retornar a posição do sensor à posiçao incial, o método de retorno será chamado
    //a quantidade de vezes indicado por esse atributo. Após isso o contador será zerado.
    private Motor motorEsquerda;
    private Motor motorDireita;
    private Motor motorSensor;
    private UltrasonicSensor sensorUltrasonico;
    private LightSensor sensorLuminosidade;
    private Comunicador comunicador;

    public LegoNXTRobo(Comunicador comunicador) {
        this.motorEsquerda = Motor.A;
        this.motorDireita = Motor.B;
        this.motorSensor = Motor.C;
        this.sensorUltrasonico = new UltrasonicSensor(SensorPort.S1);
        this.sensorLuminosidade = new LightSensor(SensorPort.S2);
        this.comunicador = comunicador;

    }

    public void andarFrente() {
        girarMotores(Configuracao.DISTANCIA, Configuracao.DISTANCIA);

    }

    public void andarTras() {
        girarMotores(-Configuracao.DISTANCIA, -Configuracao.DISTANCIA);
    }

    public void girarDireita() {
        girarMotores(Configuracao.GRAU * 90, -Configuracao.GRAU * 90);
    }

    public void girarEsquerda() {
        girarMotores(-Configuracao.GRAU * 90, Configuracao.GRAU * 90);
    }

    public void inverterSentido() {
        girarMotores((-Configuracao.GRAU * 180), (Configuracao.GRAU * 180));
    }

    public void ativarSensorUltrasonico() {
//        sensorUltrasonico.continuous();
    }

    public void desativarSensorUltrasonico() {
//        sensorUltrasonico.off();
    }

    public int getDistanciaSensorUltrasonico() {
        return sensorUltrasonico.getDistance();
    }

    public void girarSensorUltrasonicoEm(final int angulo) {
        motorSensor.rotateTo(angulo);
    }

    public void girarSensorUltrasonicoEsquerda() {
        girarSensorUltrasonicoEm(0);
    }

    public void girarSensorUltrasonicoFrente() {
        girarSensorUltrasonicoEm(90);
    }

    public void girarSensorUltrasonicoDireita() {
        girarSensorUltrasonicoEm(180);
    }

    public void ativarSensorLuminosidade() {
        sensorLuminosidade.setFloodlight(true);
    }

    public void desativarSensorLuminosidade() {
        sensorLuminosidade.setFloodlight(false);
    }

    public int getLuminosidadeSensor() {
        return sensorLuminosidade.readValue();
    }

    public void calibrar() {
        //TODO: implementar calibrar()
        throw new UnsupportedOperationException("Método calibrar() ainda não foi implementado.");
    }

    private void girarMotores(final int anguloEsquerda, final int anguloDireita) {
        motorEsquerda.rotate(anguloEsquerda, true);
        motorDireita.rotate(anguloDireita);
    }

    private void girarMotores(final double anguloEsquerda, final double anguloDireita) {
        girarMotores((int) anguloEsquerda, (int) anguloDireita);
    }

    public Comunicador getComunicador() {
        return comunicador;
    }
}